/**
 * @file rtk_scheduler.c
 * @author LokLiang (lokliang@163.com)
 * @brief
 * @version 0.1
 * @date 2023-05-01
 *
 * @copyright Copyright (c) 2023
 *
 */

#include "prv_rtk_class.h"

struct cm_rtk_struct *cm_rtk = NULL;
struct rtk_thread_handle *curr_thread_handle = NULL;
struct rtk_thread_handle *volatile next_thread_handle = NULL;

static void _rtk_read_stack_grow_up(bool *out);
static void *_port_malloc(size_t size);
static k_work_q_t *_port_get_work_q_hdl(void);
static void _port_entry_critical(void);
static void _port_exit_critical(void);

rtk_err_t rtk_scheduler_init(void *heap, size_t size)
{
    if (curr_thread_handle != NULL)
    {
        _RTK_WRN("scheduler already started");
        return RTK_FAIL;
    }

    /* heap mem */
    size_t end = (size_t)heap + size;
    heap = (void *)(((size_t)heap + sizeof(size_t) - 1) / sizeof(size_t) * sizeof(size_t));
    size = (end - (size_t)heap) / sizeof(size_t) * sizeof(size_t);

    cm_rtk = (struct cm_rtk_struct *)heap;
    heap = &cm_rtk[1];
    size -= sizeof(*cm_rtk);
    memset(cm_rtk, 0, sizeof(*cm_rtk));

    /* heap */
    if (heap_init(&cm_rtk->heap_handle, heap, size) != 0)
    {
        return RTK_NOMEM;
    }

    g_heap_default_handle = cm_rtk->heap_handle;

    /* k_kit */
    static k_init_t const init_struct = {
        .malloc = _port_malloc,
        .free = rtk_heap_free,
        .get_sys_ticks = rtk_get_sys_ticks,
        .interrupt_save = rtk_cpu_interrupt_save,
        .interrupt_restore = rtk_cpu_interrupt_restore,
        .scheduler_disable = _port_entry_critical,
        .scheduler_enable = _port_exit_critical,
        .get_work_q_hdl = _port_get_work_q_hdl,
        .thread_sleep = rtk_thread_sleep,
    };
    k_init(&init_struct);

    /* heapk mem */
    heapk_create(&cm_rtk->heapk_handle, &cm_rtk->heap_handle, 16);

    /* heap semaphore */
    _rtk_sem_init(&cm_rtk->heap_sem, 0, 1, RTK_SEM_TYPE_PRIOR);

    /* list */
    for (size_t i = 0; i < _ARRAY_COUNT(cm_rtk->list_pend); i++)
    {
        _rtk_list_init_list(&cm_rtk->list_pend[i]);
    }
    _rtk_list_init_list(&cm_rtk->list_sleep);
    slist_init_list(&cm_rtk->slist_critical);
    slist_init_list(&cm_rtk->slist_active);
    slist_init_list(&cm_rtk->slist_delete);

    /* check stack grow type */
    bool growup;
    _rtk_read_stack_grow_up(&growup);
    cm_rtk->stack_grow_up = growup;
    cm_rtk->nest_sched = 1;

    return RTK_OK;
}

rtk_err_t rtk_scheduler_start(void)
{
    if (curr_thread_handle != NULL)
    {
        _RTK_WRN("scheduler already started");
        return RTK_FAIL;
    }

    /* service: idle */
    rtk_thread_create(&cm_rtk->thread_idle,     // rtk_thread_t *thread_handle,
                      "sys-idle",               // const char *name,
                      _rtk_service_thread_idle, // void (*entry)(void *arg),
                      NULL,                     // void *arg,
                      sizeof(size_t) * 0x68,    // size_t stack_size,
                      0,                        // rtk_prior_t priority,
                      0                         // u8_t slice_ticks,
    );
    ((struct rtk_thread_handle *)cm_rtk->thread_idle.hdl)->flag |= _RTK_THREAD_FLAG_SYS;

    heap_port_init(_port_entry_critical, _port_exit_critical);

    cm_rtk->MemDetector_Tail = _RTK_HEAP_DET_VALUE;
    cm_rtk->MemDetector_Head = _RTK_HEAP_DET_VALUE;
    next_thread_handle = cm_rtk->thread_idle.hdl;
    curr_thread_handle = next_thread_handle;

    rtk_cpu_scheduler_start(next_thread_handle);

    return RTK_FAIL;
}

static void _rtk_read_stack_grow_up(bool *out)
{
    volatile int ret[0x100];
    if ((int)&ret[0x80] > (int)out)
    {
        *out = true;
    }
    else
    {
        *out = false;
    }
}

static void *_port_malloc(size_t size)
{
    return rtk_heap_malloc(size, 0);
}

static k_work_q_t *_port_get_work_q_hdl(void)
{
    _RTK_ASS_FALSE((curr_thread_handle->flag & _RTK_THREAD_FLAG_WORKQ) == 0, "curr_thread_handle is not work queue thread");
    return &curr_thread_handle->obj.work_q_handle;
}

static void _port_entry_critical(void)
{
    if (CONFIG_RTK_ISR_SUPPORT == 1)
    {
        rtk_int_save();
    }
    else
    {
        rtk_sched_suspend();
    }
}

static void _port_exit_critical(void)
{
    if (CONFIG_RTK_ISR_SUPPORT == 1)
    {
        rtk_int_restore_auto();
    }
    else
    {
        rtk_sched_resume_auto();
    }
}

void rtk_int_entry(void)
{
    _RTK_ASS_FALSE(cm_rtk == NULL, "Never use rtk_scheduler_init() to initialize kernel");
    size_t nest_int = rtk_cpu_interrupt_save();
    ++cm_rtk->flag_int;
    rtk_cpu_interrupt_restore(nest_int);
}

void rtk_int_exit(void)
{
    _RTK_ASS_FALSE(cm_rtk == NULL, "Never use rtk_scheduler_init() to initialize kernel");
    size_t nest_int = rtk_cpu_interrupt_save();
    cm_rtk->flag_int -= !!cm_rtk->flag_int;
    rtk_cpu_interrupt_restore(nest_int);
}

bool rtk_is_isr_context(void)
{
    if (cm_rtk->flag_int)
    {
        return true;
    }
    return rtk_cpu_is_isr();
}

size_t rtk_sched_suspend(void)
{
    size_t nest_int = rtk_cpu_interrupt_save();
    size_t nest = cm_rtk->nest_sched++;
    rtk_cpu_interrupt_restore(nest_int);
    return nest;
}

void rtk_sched_resume(size_t nest)
{
    size_t nest_int = rtk_cpu_interrupt_save();
    cm_rtk->nest_sched = nest;
    rtk_cpu_interrupt_restore(nest_int);
    if (nest == 0)
    {
        if (curr_thread_handle != next_thread_handle)
        {
            _rtk_scheduler_to_highest();
        }
    }
}

void rtk_sched_resume_auto(void)
{
    size_t nest_int = rtk_cpu_interrupt_save();
    size_t nest = (cm_rtk->nest_sched -= !!cm_rtk->nest_sched);
    rtk_cpu_interrupt_restore(nest_int);
    if (nest == 0)
    {
        if (curr_thread_handle != next_thread_handle)
        {
            _rtk_scheduler_to_highest();
        }
    }
}

size_t rtk_int_save(void)
{
    cm_rtk->nest_int = rtk_cpu_interrupt_save();
    return cm_rtk->nest_int++;
}

void rtk_int_restore(size_t nest)
{
    cm_rtk->nest_int = nest;
    rtk_cpu_interrupt_restore(nest);
}

void rtk_int_restore_auto(void)
{
    if (cm_rtk->nest_int != 0)
    {
        cm_rtk->nest_int--;
        rtk_int_restore(cm_rtk->nest_int);
    }
}

rtk_scheduler_state_t rtk_scheduler_state(void)
{
    if (curr_thread_handle == NULL)
    {
        return RTK_SCHED_STATE_NOTSTART;
    }
    else
    {
        if (cm_rtk->nest_sched == 0)
        {
            return RTK_SCHED_STATE_RUNNING;
        }
        else
        {
            return RTK_SCHED_STATE_SUSPEND;
        }
    }
}

/**
 * @brief 置起 位-值 表对应值的位
 *
 * @param index 位-值 表
 * @param lev 值
 */
void _rtk_prior_set(struct rtk_prior_index *index, size_t lev)
{
    _RTK_ASS_FALSE(lev > CONFIG_RTK_MAX_PRIORITY, "lev = %d", lev);

#if defined(__CC_ARM) || defined(__GNUC__)

#if CONFIG_RTK_MAX_PRIORI > 31 // 优先级高于 31 时需要 2 维或以上的 tab

    size_t y = lev / (8 * sizeof(index->tab[0]));
    index->tab[y] |= 1 << (lev & 0x1F);
    index->tab_y |= 1 << y;

#else // #if CONFIG_RTK_MAX_PRIORI > 31

    index->tab[0] |= 1 << lev;

#endif // #if CONFIG_RTK_MAX_PRIORI > 31

#else // #if defined(__CC_ARM) || defined(__GNUC__)

    size_t y = lev / (8 * sizeof(index->tab[0]));
    index->tab[y] |= 1 << (lev & 0x07);
    index->tab_y |= 1 << y;

#endif // #if defined(__CC_ARM) || defined(__GNUC__)
}

/**
 * @brief 清除 位-值 表对应值的位
 *
 * @param index 位-值 表
 * @param lev 值
 */
void _rtk_prior_reset(struct rtk_prior_index *index, size_t lev)
{
    _RTK_ASS_FALSE(lev > CONFIG_RTK_MAX_PRIORITY, "lev = %d", lev);

#if defined(__CC_ARM) || defined(__GNUC__)

#if CONFIG_RTK_MAX_PRIORI > 31 // 优先级高于 31 时需要 2 维或以上的 tab

    int y = lev / (8 * sizeof(index->tab[0]));
    index->tab[y] &= ~(1u << (lev & 0x1F));
    if (index->tab[y] == 0)
    {
        index->tab_y &= ~(1u << y);
    }

#else // #if CONFIG_RTK_MAX_PRIORI > 31

    index->tab[0] &= ~(1u << lev);

#endif // #if CONFIG_RTK_MAX_PRIORI > 31

#else // #if defined(__CC_ARM) || defined(__GNUC__)

    int y = lev / (8 * sizeof(index->tab[0]));
    index->tab[y] &= ~(1u << (lev & 0x07));
    if (index->tab[y] == 0)
    {
        index->tab_y &= ~(1u << y);
    }

#endif // #if defined(__CC_ARM) || defined(__GNUC__)
}

/**
 * @brief 查询 位-值 表中的记录到的最高值
 *
 * @param index 位-值 表
 * @return lev 0..n: 记录到的最高值；~0: 无任何记录
 */
size_t _rtk_prior_highest(struct rtk_prior_index *index)
{
#if defined(__CC_ARM) || defined(__GNUC__)

#if CONFIG_RTK_MAX_PRIORI > 31 // 优先级高于 31 时需要 2 维或以上的 tab

    int y = 31 - _cpu_clz(index->tab_y);
    if (y < 0)
    {
        return ~0;
    }

    size_t x = 31 - _cpu_clz(index->tab[y]);
    return (y * 32 + x);

#else // #if CONFIG_RTK_MAX_PRIORI > 31

    size_t x = 31 - _cpu_clz(index->tab[0]);
    return x;

#endif // #if CONFIG_RTK_MAX_PRIORI > 31

#else // #if defined(__CC_ARM) || defined(__GNUC__)

    static char const _tab_clz8[256] = {
        /* 0x00 */ 0, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, //
        /* 0x10 */ 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, //
        /* 0x20 */ 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, //
        /* 0x30 */ 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, //
        /* 0x40 */ 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, //
        /* 0x50 */ 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, //
        /* 0x60 */ 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, //
        /* 0x70 */ 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, //
        /* 0x80 */ 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, //
        /* 0x90 */ 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, //
        /* 0xA0 */ 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, //
        /* 0xB0 */ 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, //
        /* 0xC0 */ 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, //
        /* 0xD0 */ 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, //
        /* 0xE0 */ 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, //
        /* 0xF0 */ 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, //
    };

    if (index->tab_y == 0)
    {
        return ~0;
    }

    size_t y = _tab_clz8[index->tab_y];
    size_t x = _tab_clz8[index->tab[y]];
    return (y * 8 + x);

#endif // #if defined(__CC_ARM) || defined(__GNUC__)
}

/**
 * @brief 执行上下文切换。中断和线程都可用。可重入。
 */
void _rtk_scheduler_to_highest(void)
{
    size_t nest_int = rtk_cpu_interrupt_save();

    struct rtk_thread_handle *thread_from = curr_thread_handle;
    struct rtk_thread_handle *thread_to = next_thread_handle;

    if (cm_rtk->nest_sched == 0 && thread_from != thread_to)
    {
        _rtk_scheduler_do_swap(thread_from, thread_to);
    }

    rtk_cpu_interrupt_restore(nest_int);
}

void _rtk_scheduler_do_swap(struct rtk_thread_handle *from, struct rtk_thread_handle *to)
{
    curr_thread_handle = to;
    rtk_cpu_switch_to(from, to);

#if defined(CONFIG_RTK_SWAP_HOOK_FN)
    if (!(to->flag & _RTK_THREAD_FLAG_SYS))
    {
        extern void CONFIG_RTK_SWAP_HOOK_FN(const char *thread_name);
        CONFIG_RTK_SWAP_HOOK_FN(rtk_heapk_key_base(to));
    }
#endif
}

__weak void CONFIG_RTK_SWAP_HOOK_FN(const char *thread_name)
{
    _RTK_UNUSE(thread_name);
}
